#ifndef CQWU_CAR_CONTROL
#define CQWU_CAR_CONTROL

#include <stdbool.h>
#include <float.h>
#include <stdint.h>  // 新增标准类型支持

// 控制参数常量
#define MAX_STEER_ANGLE (0.7854f) // 45度(弧度)
#define MIN_SPEED 0.2f            // 最小行驶速度(m/s)

// 新增PID控制器结构
typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
    float output;
} PIDController;

// 传感器数据结构
typedef struct {
    float heading;
    float speed;
    bool valid;  // 数据有效性标志
} GPSData;

typedef struct {
    float lane_offset;
    bool lane_detected;
} VisionData;

// 车辆状态结构
typedef struct {
    float position[2];
    float heading;
    float speed;
} VehicleState;

// 执行器命令
typedef struct {
    float steer;
    float throttle;
} ActuatorCommand;

// 显示尺寸定义
#define LCDH 120       // LCD显示高度(像素)
#define LCDW 188       // LCD显示宽度(像素)

// 地图尺寸定义
#define MAP_WIDTH 20    // 地图宽度(格子数)
#define MAP_HEIGHT 20   // 地图高度(格子数)

// 新增路径数据结构体
typedef struct {
    Node* path;         // 存储路径节点数组
    int path_length;    // 路径节点数量
    int current_index;  // 当前跟踪的路径点索引
} PathData;

// 轨迹控制参数结构体(增加动态权重支持)
typedef struct {
    float gps_weight;    // GPS基础权重(0-1)
    float cam_weight;    // 摄像头基础权重(0-1)
    float imu_weight;    // IMU基础权重(0-1)
    float max_angle;     // 最大转向角(弧度)
    float path_weight;   // 新增路径跟踪权重(0-1)
} TrajectoryParams;

// 路径节点结构体(保持不变)
typedef struct Node {
    int x, y;           // 节点坐标
    int f, g, h;        // A*算法参数
    bool walkable;      // 是否可通行
    struct Node* parent;// 父节点指针
} Node;

// 开放列表节点结构体(保持不变)
typedef struct OpenListNode {
    Node* node;         // 指向路径节点
    struct OpenListNode* next;
} OpenListNode;

// 全局变量声明(新增路径数据)
extern Node map[MAP_HEIGHT][MAP_WIDTH];
extern OpenListNode* openList;
extern bool closedList[MAP_HEIGHT][MAP_WIDTH];
extern PathData global_path;  // 新增全局路径数据
// 新增全局变量声明
extern VehicleState current_pos;  // 当前车辆位置
extern TrajectoryParams traj_params; // 轨迹参数
extern NavigationController nav_ctrl; // 导航控制器
extern MotionController motion_ctrl; // 运动控制器

// 核心算法函数声明(修改A*函数返回类型)
PathData* AStarPathFinding(Node* start, Node* end);  // 修改返回类型
float PathTracking(float current_pos[2], float lookahead);  // 新增路径跟踪函数

// 传感器控制函数声明(保持不变)
int Motro(float speed, float angle);
int imu(float* imu_data);
int GPS(float* gps_data);

// 数据处理函数声明(参数增加置信度指标)
void TrajectoryCorrection(float* gps_traj, float* imu_traj, float* cam_traj,
                         TrajectoryParams* params, float* final_angle, float* final_speed);
void ProcessCameraData(unsigned char Pixle[LCDH][LCDW], float* cam_traj);

// 新增导航控制接口
void Navigation_Init(void);
bool Navigation_UpdatePath(Node start, Node goal);
float Navigation_GetSteeringAngle(VehicleState state);

// 增强传感器融合接口
void SensorFusion_UpdateWeights(float gps, float cam, float imu);
void SensorFusion_Process(GPSData gps, VisionData vision, VehicleState* output);

// 运动控制接口
void MotionControl_Init(PIDConfig speed_pid, PIDConfig angle_pid);
void MotionControl_Update(VehicleState state, float target_speed, ActuatorCommand* cmd);

#endif
